// 1.包含头文件
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32.hpp"
#include "std_msgs/msg/float64.hpp"
#include <cmath>
#include <iostream>
#include <Eigen/Dense>
// 在此分别设臂长为a,b,c 且a与垂直面夹角为alpha
float a, b, c, alpha, x_, y_, z_, t1, t2, t3, theta_1, theta_2, theta_3, p_x, p_y, p_z;
float theta;
float l21, l22, l23, l31, l32, l33;
// 最初状态下的长度，用来求初始角度
float initl2, initl3;
// qt中传来的坐标值
int qtx_, qty_, qtz_;

void IK_RESULT(const float x, const float y, const float z, float &theta1, float &theta2, float &theta3);
void FK_RESULT(const float theta1_, const float theta2_, const float theta3_, float &px_, float &py_, float &pz_);
void to_cos(const float c_a, const float c_b, const float c_c, float &theta_c);
void to_length(const float l_a, const float l_b, const float l_theta_c, float &l_c);

using namespace std;
using namespace Eigen;

// 3.自定义节点类；
class Ik : public rclcpp::Node
{
public:
    Ik() : Node("ik_node_cpp")
    {
        // 实例化回调组
        callback_group_0 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
        callback_group_1 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
        callback_group_2 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
        callback_group_3 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);

        // 创建订阅选项，并设置回调组
        rclcpp::SubscriptionOptions options_0;
        options_0.callback_group = callback_group_0;
        rclcpp::SubscriptionOptions options_1;
        options_1.callback_group = callback_group_1;
        rclcpp::SubscriptionOptions options_2;
        options_2.callback_group = callback_group_2;
        // 创建订阅方
        this->qtx_sub_ = this->create_subscription<std_msgs::msg::Int32>("qtx", 5, std::bind(&Ik::qtx_cb, this, std::placeholders::_1), options_0);
        this->qty_sub_ = this->create_subscription<std_msgs::msg::Int32>("qty", 5, std::bind(&Ik::qty_cb, this, std::placeholders::_1), options_1);
        this->qtz_sub_ = this->create_subscription<std_msgs::msg::Int32>("qtz", 5, std::bind(&Ik::qtz_cb, this, std::placeholders::_1), options_2);

        timer_ = this->create_wall_timer(0.02s, std::bind(&Ik::timer_cb, this), callback_group_3);
        // while (1)
        // {
        //     alpha = 10 * M_PI / 180;
        //     std::cin >> x_ >> y_ >> z_;
        //     IK_RESULT(x_, y_, z_, t1, t2, t3);
        //     std::cout << t1 << "\n"
        //               << std::endl;
        //     std::cout << t2 << "\n"
        //               << std::endl;
        //     std::cout << t3 << "\n"
        //               << std::endl;
        //     // 这里输入的是几何夹角
        //     std::cin >> theta_1 >> theta_2 >> theta_3;
        //     theta_2 = 90 - theta_2;
        //     theta_3 = 180 - theta_3;
        //     std::cout << theta_1 << theta_2 << theta_3 << std::endl;
        //     FK_RESULT(theta_1, theta_2, theta_3, p_x, p_y, p_z);
        //     cout << "p_x = \n"
        //          << p_x << endl;
        //     cout << "p_y = \n"
        //          << p_y << endl;
        //     cout << "p_z = \n"
        //          << p_z << endl;
        // }
    }

private:
    // 订阅来自QT的x消息
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr qtx_sub_;
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr qty_sub_;
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr qtz_sub_;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr joint2adc_sub_;
    rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr joint3adc_sub_;

    // 定时函数
    rclcpp::TimerBase::SharedPtr timer_;
    // 声明回调组
    rclcpp::CallbackGroup::SharedPtr callback_group_0;
    rclcpp::CallbackGroup::SharedPtr callback_group_1;
    rclcpp::CallbackGroup::SharedPtr callback_group_2;
    rclcpp::CallbackGroup::SharedPtr callback_group_3;
    // 初始量
    float tempx,tempy,tempz;

    // 订阅x y z 坐标的回调函数
    void qtx_cb(const std_msgs::msg::Int32::SharedPtr qtx_msg)
    {
        qtx_ = qtx_msg->data;
    }
    void qty_cb(const std_msgs::msg::Int32::SharedPtr qty_msg)
    {
        qty_ = qty_msg->data;
    }
    void qtz_cb(const std_msgs::msg::Int32::SharedPtr qtz_msg)
    {
        qtz_ = qtz_msg->data;
    }
    void timer_cb()
    {
        IK_RESULT(qtx_, qty_, qtz_, theta_1, theta_2, theta_3);
        // 求第二关节液压缸行程
        to_length(l21, l22, theta_2, l23);
        // 求第三关节液压缸行程
        to_length(l31, l32, theta_3, l33);
        // 这里需要发送给arm_joint控制液压缸行程，此处计算出的是根据QT中坐标位置计算得到的液压缸行程
    }
};

int main(int argc, char const *argv[])
{
    a = 1230;
    b = 1740;
    c = 2300;
    // 2.初始化ROS2客户端；
    rclcpp::init(argc, argv);
    rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 20, true);
    // 4.调用spain函数，并传入节点对象指针；
    auto iknode = std::make_shared<Ik>();
    executor.add_node(iknode);
    executor.spin();
    // 5.资源释放
    rclcpp::shutdown();
    return 0;
}

// 逆运动学解算
// 设要到达的位置为 x, y, z
void IK_RESULT(const float x, const float y, const float z, float &theta1, float &theta2, float &theta3)
{
    // 底座角
    theta1 = atan(x / y);

    // 大臂角
    float projection = sqrt(x * x + y * y);
    float high = 1231.0097;
    if (high > z)
    {
        float short_ = high - z;
        float slant_ = sqrt(short_ * short_ + projection * projection);
        float theta2_1 = atan(projection / short_);
        float theta2_2 = acos((b * b + slant_ * slant_ - c * c) / (2 * b * slant_));
        theta2 = theta2_1 + theta2_2;
        // std::cout << "high > z";
    }
    else if (high < z)
    {
        float short_ = z - high;
        float slant_ = sqrt(short_ * short_ + projection * projection);
        float theta2_1 = atan(short_ / projection);
        float theta2_2 = acos((b * b + slant_ * slant_ - c * c) / (2 * b * slant_));
        theta2 = theta2_1 + theta2_2;
        // std::cout << "high < z ";
    }
    else
    {
        float short_ = z - high;
        float slant_ = sqrt(short_ * short_ + projection * projection);
        float theta2_2 = acos((b * b + slant_ * slant_ - c * c) / (2 * b * slant_));
        theta2 = theta2_2;
        // std::cout << "high = z ";
    }
    // 小臂角
    float short_2 = std::abs(z - high);
    float slant_2 = sqrt(short_2 * short_2 + projection * projection);
    theta3 = acos((b * b + c * c - slant_2 * slant_2) / (2 * b * c));
    // 转换成角度制
    theta1 = theta1 * (180 / M_PI);
    theta2 = theta2 * (180 / M_PI);
    theta3 = theta3 * (180 / M_PI);
}

void FK_RESULT(const float theta1_, const float theta2_, const float theta3_, float &px_, float &py_, float &pz_)
{
    // , int &x, int &y, int &z
    // float theta2 = 90 - theta_2;
    // float theta3 = 180 - theta_3;
    float theta1, theta2, theta3;
    theta1 = -theta1_ / 180 * M_PI;
    theta2 = (-theta2_ / 180 * M_PI);
    theta3 = (-theta3_ / 180 * M_PI);

    Matrix4d T01;
    T01 << cos(theta1), -sin(theta1) * cos(-M_PI / 2), sin(theta1) * sin(-M_PI / 2), 0,
        sin(theta1), cos(theta1) * cos(-M_PI / 2), -cos(theta1) * sin(-M_PI / 2), 0,
        0, sin(-M_PI / 2), cos(-M_PI / 2), 1231,
        0, 0, 0, 1;

    // 定义矩阵 T12
    Matrix4d T12;
    T12 << cos(theta2), -sin(theta2) * cos(0), sin(theta2) * sin(0), b * cos(theta2),
        sin(theta2), cos(theta2) * cos(0), -cos(theta2) * sin(0), b * sin(theta2),
        0, sin(0), cos(0), 0,
        0, 0, 0, 1;

    // 定义矩阵 T23
    Matrix4d T23;
    T23 << cos(theta3), -sin(theta3) * cos(0), sin(theta3) * sin(0), c * cos(theta3),
        sin(theta3), cos(theta3) * cos(0), -cos(theta3) * sin(0), c * sin(theta3),
        0, sin(0), cos(0), 0,
        0, 0, 0, 1;

    // 计算 T02 = T01 * T12
    Matrix4d T02 = T01 * T12;

    // 计算 T03 = T02 * T23
    Matrix4d T03 = T02 * T23;

    px_ = T03(0, 3);
    py_ = T03(1, 3);
    pz_ = T03(2, 3);
}
// 已知三角形的三边 a、b 和 c，要求夹角 C
void to_cos(const float c_a, const float c_b, const float c_c, float &theta_c)
{
    float cosc = (c_a * c_a + c_b * c_b - c_c * c_c) / (2 * c_a * c_b);
    float c_rad = acos(cosc);
    float c_deg = c_rad * 180.0 / M_PI;
    theta_c = c_deg;
}
// 已知三角形的两边 a 和 b 以及它们之间的夹角 c（以度为单位），要求第三边 c。
void to_length(const float l_a, const float l_b, const float l_theta_c, float &l_c)
{
    float c_rad = l_theta_c * M_PI / 180.0;
    float c = sqrt(l_a * l_a + l_b * l_b - 2 * l_a * l_b * cos(c_rad));
    l_c = c;
}


/*
    2025 02 10
    需要通过 to_cos() 函数获取初始状态下的各关节角度，随后根据正运动学计算初始XYZ坐标，此后QT在此基础上进行增减操作
*/